

#include "LegacyMotionReaderXML.h"
#include "../../XMLTools.h"
#include "../../Model/ModelProcessorFactory.h"
#include "../../Model/ModelReaderXML.h"

#include <algorithm>
#include <fstream>

#include <filesystem>


#include "../../RapidXML/rapidxml.hpp"

using std::cout;
using std::endl;

namespace MMM
{

    LegacyMotionReaderXML::LegacyMotionReaderXML()
    {
        registerMotionXMLTag("comments", this);
        registerMotionXMLTag("model", this);
        registerMotionXMLTag("modelprocessorconfig", this);
        registerMotionXMLTag("jointorder", this);
        registerMotionXMLTag("motionframes", this);
        registerMotionFrameXMLTag("TimeStep", this);
        registerMotionFrameXMLTag("RootPosition", this);
        registerMotionFrameXMLTag("RootPositionVelocity", this);
        registerMotionFrameXMLTag("RootPositionAcceleration", this);
        registerMotionFrameXMLTag("RootRotation", this);
        registerMotionFrameXMLTag("RootRotationVelocity", this);
        registerMotionFrameXMLTag("RootRotationAcceleration", this);
        registerMotionFrameXMLTag("JointPosition", this);
        registerMotionFrameXMLTag("JointVelocity", this);
        registerMotionFrameXMLTag("JointAcceleration", this);
    }

    LegacyMotionPtr LegacyMotionReaderXML::loadMotion(const std::string& xmlFile, const std::string& motionName)
    {

        // load file
        std::ifstream in(xmlFile.c_str());

        if (!in.is_open())
        {
            MMM_ERROR << "Could not open XML file:" << xmlFile << endl;
            return LegacyMotionPtr();
        }

        std::stringstream buffer;
        buffer << in.rdbuf();
        std::string motionXML(buffer.str());
        in.close();

        LegacyMotionPtr res = createMotionFromString(motionXML, motionName, xmlFile);
        if (!res)
        {
            MMM_ERROR << "Error while parsing file " << xmlFile << endl;
        }

        return res;
    }

    LegacyMotionList LegacyMotionReaderXML::loadAllMotions(const std::string& xmlFile)
    {
        std::vector < std::string > motionNames = getMotionNames(xmlFile);

        if (motionNames.size() == 0)
        {
            MMM_ERROR << "no motions in file " << endl;
            return std::vector<LegacyMotionPtr>();
        }

        std::vector<LegacyMotionPtr> result;

        int frameCount = -1;
        for (const auto& motionName : motionNames)
        {
            MMM::LegacyMotionPtr motion = loadMotion(xmlFile, motionName);
            if (!motion)
            {
                continue;
            }
            MMM_INFO << "Loading motion: " << motion->getName() << endl;
            if (frameCount != -1 && static_cast<std::size_t>(frameCount) != motion->getNumFrames())
            {
                MMM_ERROR << "Error whole loading motions: the frame count between two motions do not match. All motions need to have the same framecount." << endl;
                continue;
            }
            else
            {
                frameCount = motion->getNumFrames();
            }
            result.push_back(motion);
        }

        return result;
    }

    LegacyMotionList LegacyMotionReaderXML::loadAllMotionsFromString(const std::string& xmlDataString)
    {
        std::vector < std::string > motionNames = getMotionNamesFromXMLString(xmlDataString);

        if (motionNames.size() == 0)
        {
            MMM_ERROR << "no motions in file " << endl;
            return std::vector<LegacyMotionPtr>();
        }

        std::vector<LegacyMotionPtr> result;

        int frameCount = -1;
        for (const auto& motionName : motionNames)
        {
            MMM::LegacyMotionPtr motion = createMotionFromString(xmlDataString, motionName);
            if (!motion)
            {
                continue;
            }
            MMM_INFO << "Loading motion: " << motion->getName() << endl;
            if (frameCount != -1 && static_cast<std::size_t>(frameCount) != motion->getNumFrames())
            {
                MMM_ERROR << "Error whole loading motions: the frame count between two motions do not match. All motions need to have the same framecount." << endl;
                continue;
            }
            else
            {
                frameCount = motion->getNumFrames();
            }
            result.push_back(motion);
        }

        return result;
    }

    std::vector<std::string> LegacyMotionReaderXML::getMotionNamesFromXMLString(std::string motionXML) const
    {
        std::vector<std::string> res;
        try
        {
            rapidxml::xml_document<char> doc;   // character type defaults to char
            char* y = doc.allocate_string(motionXML.c_str());
            doc.parse<0>(y);                    // 0 means default parse flags

            rapidxml::xml_node<>* node = doc.first_node(XML::MOTION_XML_ROOT.c_str(), 0, false);
            if (node)
            {
                node = node->first_node("motion", 0, false);
            }
            else
            {
                MMM_ERROR << "Could not find " << XML::MOTION_XML_ROOT << " node" << endl;
            }

            while (node)
            {
                rapidxml::xml_attribute<>* attr = node->first_attribute("name");
                if (attr)
                {
                    res.push_back(attr->value());
                }
                else
                {
                    MMM_WARNING << "Found Motion without name-attribute - skipping" << endl;
                }
                node = node->next_sibling("motion", 0, false);
            }
        }
        catch (rapidxml::parse_error& e)
        {

            MMM_ERROR << "Could not parse data in XML definition" << endl
                      << "Error message:" << e.what() << endl
                      << "Position: " << endl << e.where<char>() << endl;
            return res;
        }
        catch (std::exception& e)
        {

            MMM_ERROR << "Error while parsing XML definition" << endl
                      << "Error code:" << e.what() << endl;
            return res;
        }
        catch (...)
        {

            MMM_ERROR << "Error while parsing XML definition" << endl;
            return res;
        }
        return res;

    }

    std::vector<std::string> LegacyMotionReaderXML::getMotionNames(const std::string& xmlFile) const
    {
        // load file
        std::ifstream in(xmlFile.c_str());
        std::vector<std::string> res;
        if (!in.is_open())
        {
            MMM_ERROR << "Could not open XML file:" << xmlFile << endl;
            return res;
        }

        std::stringstream buffer;
        buffer << in.rdbuf();
        std::string motionXML(buffer.str());
        in.close();
        res = getMotionNamesFromXMLString(motionXML);


        return res;
    }


    LegacyMotionPtr LegacyMotionReaderXML::createMotionFromString(const std::string& xmlString, const std::string& motionName, const std::string& motionFilePath)
    {
        try
        {
            rapidxml::xml_document<char> doc;   // character type defaults to char
            char* y = doc.allocate_string(xmlString.c_str());
            doc.parse<0>(y);                    // 0 means default parse flags

            // get <MMM> tag
            rapidxml::xml_node<>* node = doc.first_node(XML::MOTION_XML_ROOT.c_str(), 0, false);
            if (node)
            {
                node = node->first_node();
            }
            else
            {
                MMM_ERROR << "Could not find " << XML::MOTION_XML_ROOT << " node" << endl;
                return LegacyMotionPtr();
            }


            // process <Motion> tags
            while (node)
            {
                std::string nodeName = XML::toLowerCase(node->name());
                if (nodeName == "motion")
                {
                    rapidxml::xml_attribute<>* attr = node->first_attribute("name");
                    if (motionName.empty() || (attr && attr->value() == motionName))
                    {
                        // we found the entry
                        node = node->first_node();

                        break;
                    }

                }

                node = node->next_sibling();
            }
            if (!node)
            {
                MMM_ERROR << "Could not find Motion with name " << motionName << ", or no Motion defined" << endl;
                return LegacyMotionPtr();
            }

            LegacyMotionPtr motion(new LegacyMotion(motionName));
            motion->setMotionFilePath(MMM::XML::getPath(motionFilePath));
            motion->setMotionFileName(MMM::XML::getFileName(motionFilePath)); //added to be able to track the original motion file

            // now process the motion data set entries
            while (node)
            {
                std::string nodeName = XML::toLowerCase(node->name());
                if (xmlProcessors.find(nodeName) != xmlProcessors.end())
                {
                    if (!xmlProcessors[nodeName]->processMotionXMLTag(node, motion))
                    {
                        MMM_ERROR << "Error while processing XML tag <" << nodeName << ">" << endl;
                        return LegacyMotionPtr();
                    }
                }
                else
                {
                    //MMM_INFO << "Ignoring unknown XML tag <" << nodeName << ">" << endl;
                }
                node = node->next_sibling();
            }
            if (motion->getModelProcessor() && motion->getModel(false) && ((motion->getModel(true) == motion->getModel(false)) || !motion->getModel(true)))
            {
                ModelPtr mOrig = motion->getModel(false);
                ModelPtr mProcessed = motion->getModelProcessor()->convertModel(mOrig);
                motion->setModel(mProcessed, mOrig);
            }

            //motion->calculateVelocities();
            //motion->calculateAccelerations();


            return motion;
        }
        catch (rapidxml::parse_error& e)
        {
            MMM_ERROR << "Could not parse data in XML definition" << endl
                      << "Error message:" << e.what() << endl
                      << "Position: " << endl << e.where<char>() << endl;
        }
        catch (std::exception& e)
        {
            MMM_ERROR << "Error while parsing XML definition" << endl
                      << "Error code:" << e.what() << endl;
        }
        catch (...)
        {
            MMM_ERROR << "Error while parsing XML definition" << endl;
        }

        return LegacyMotionPtr();
    }

    bool LegacyMotionReaderXML::processMotionXMLTag(rapidxml::xml_node<char>* tag, LegacyMotionPtr motion)
    {
        if (!tag)
        {
            return false;
        }
        std::string name = XML::toLowerCase(tag->name());
        if (name == "comments")
        {
            return processCommentsTag(tag, motion);
        }
        if (name == "jointorder")
        {
            return processJointOrderTag(tag, motion);
        }
        if (name == "model")
        {
            return processModelTag(tag, motion);
        }
        if (name == "modelprocessorconfig")
        {
            return processModelProcessorConfigTag(tag, motion);
        }

        if (name == "motionframes")
        {
            return processMotionFramesTag(tag, motion);
        }

        MMM_ERROR << "Unknown XML tag:" << tag->name() << endl;
        return false;
    }


    bool LegacyMotionReaderXML::processMotionFrameXMLTag(rapidxml::xml_node<char>* tag, MotionFramePtr motionFrame)
    {
        if (!tag || !motionFrame)
        {
            return false;
        }
        bool res = false;
        std::string name = XML::toLowerCase(tag->name());


        if (name == "rootposition")
        {
            res = motionFrame->setRootPos(XML::getFloatArray(tag->value(), 3));
        }
        else if (name == "rootpositionvelocity")
        {
            res = motionFrame->setRootPosVel(XML::getFloatArray(tag->value(), 3));
        }
        else if (name == "rootpositionacceleration")
        {
            res = motionFrame->setRootPosAcc(XML::getFloatArray(tag->value(), 3));
        }
        else if (name == "rootrotation")
        {
            res = motionFrame->setRootRot(XML::getFloatArray(tag->value(), 3));
        }
        else if (name == "rootrotationvelocity")
        {
            res = motionFrame->setRootRotVel(XML::getFloatArray(tag->value(), 3));
        }
        else if (name == "rootrotationacceleration")
        {
            res = motionFrame->setRootRotAcc(XML::getFloatArray(tag->value(), 3));
        }
        else if (name == "timestep")
        {
            motionFrame->timestep = XML::convertToFloat(tag->value());
            res = true;
        }
        else if (name == "jointposition")
        {
            motionFrame->joint = XML::getFloatArray(tag->value());
            res = true;
        }
        else if (name == "jointvelocity")
        {
            motionFrame->joint_vel = XML::getFloatArray(tag->value());
            res = true;
        }
        else if (name == "jointacceleration")
        {
            motionFrame->joint_acc = XML::getFloatArray(tag->value());
            res = true;
        }
        else
        {
            //MMM_INFO << "Ignoring unknown XML motion-data tag:" << tag->name() << endl;
            // return true, this is not an error, we just ignore this tag
            return true;
        }
        return res;
    }


    bool LegacyMotionReaderXML::processCommentsTag(rapidxml::xml_node<char>* tag, LegacyMotionPtr motion)
    {
        if (!tag)
        {
            MMM_ERROR << "NULL comments tag in XML definition" << endl;
            return false;
        }
        if (!motion)
        {
            MMM_ERROR << "NULL motion ?!" << endl;
            return false;
        }
        CommentEntryPtr r(new CommentEntry());
        rapidxml::xml_node<>* node = tag->first_node("text", 0, false);
        while (node)
        {
            std::string text = node->value();
            r->comments.push_back(text);
            node = node->next_sibling("text", 0, false);
        }
        motion->addEntry("comments", r);
        return true;
    }


    bool LegacyMotionReaderXML::processModelTag(rapidxml::xml_node<char>* tag, LegacyMotionPtr motion)
    {
        if (!tag)
        {
            MMM_ERROR << "NULL comments tag in XML definition" << endl;
            return false;
        }
        if (!motion)
        {
            MMM_ERROR << "NULL motion ?!" << endl;
            return false;
        }

        rapidxml::xml_node<>* node = tag->first_node("file", 0, false);
        if (node)
        {
            std::string filename = node->value();
            ModelReaderXMLPtr mr(new ModelReaderXML());
            std::filesystem::path motionpath(motion->getMotionFilePath());
            std::filesystem::path filenamePath(filename);
            std::filesystem::path result = motionpath;
            result /= filenamePath;
            filename = std::filesystem::absolute(result).string();
            if (!XML::isValidFile(filename))
            {
                // check absolute filename
                filename = node->value();
                if (!XML::isValidFile(filename))
                {
                    MMM_WARNING << "Could not determine valid filename:" << filename << ", motion base path:" << motion->getMotionFilePath() << endl;
                }
            }

            motion->setModelFilePath(filename);
            ModelPtr m = mr->loadModel(filename);
            if (!m)
            {
                MMM_WARNING << "Could not load model from file " << filename << endl;
            }
            else
            {
                motion->setModel(m, m);
            }
        }

        // we allow failed instantiations
        return true;
    }

    bool LegacyMotionReaderXML::processModelProcessorConfigTag(rapidxml::xml_node<char>* tag, LegacyMotionPtr motion)
    {
        if (!tag)
        {
            MMM_ERROR << "NULL comments tag in XML definition" << endl;
            return false;
        }
        if (!motion)
        {
            MMM_ERROR << "NULL motion ?!" << endl;
            return false;
        }

        ModelProcessorPtr mp = ModelProcessorFactory::getModelProcessorFromNode(tag);
        if (!mp)
        {
            MMM_WARNING << "Could not instantiate model processor" << endl;
        }
        else
        {
            motion->setModelProcessor(mp);
        }


        // we allow failed instantiations
        return true;
    }



    bool LegacyMotionReaderXML::processJointOrderTag(rapidxml::xml_node<char>* tag, LegacyMotionPtr motion)
    {
        if (!tag)
        {
            MMM_ERROR << "NULL joint_order tag in XML definition" << endl;
            return false;
        }
        if (!motion)
        {
            MMM_ERROR << "NULL motion ?!" << endl;
            return false;
        }

        std::vector<std::string> names;
        rapidxml::xml_node<>* node = tag->first_node("joint", 0, false);
        while (node)
        {
            rapidxml::xml_attribute<>* attr = node->first_attribute("name", 0, false);
            if (!attr)
            {
                MMM_ERROR << "Joint_order tag: <joint> needs attribute name" << endl;
                return false;
            }
            std::string jn = attr->value();
            if (jn.empty())
            {
                MMM_ERROR << "Joint_order tag: <joint> needs not null attribute name" << endl;
                return false;
            }
            names.push_back(jn);
            node = node->next_sibling("joint", 0, false);
        }
        motion->setJointOrder(names);
        return true;
    }

    bool LegacyMotionReaderXML::processMotionFramesTag(rapidxml::xml_node<char>* tag, LegacyMotionPtr motion)
    {
        if (!tag)
        {
            MMM_ERROR << "NULL motion tag in XML definition" << endl;
            return false;
        }
        if (!motion)
        {
            MMM_ERROR << "NULL motion ?!" << endl;
            return false;
        }

        // cycle through MotionFrame tags
        rapidxml::xml_node<>* node = tag->first_node("motionframe", 0, false);
        while (node)
        {
            processMotionFrameTag(node, motion);
            node = node->next_sibling("motionframe", 0, false);
        }

        return true;
    }

    bool LegacyMotionReaderXML::processMotionFrameTag(rapidxml::xml_node<char>* tag, LegacyMotionPtr motion)
    {
        if (!tag)
        {
            MMM_ERROR << "NULL motion data tag in XML definition" << endl;
            return false;
        }
        if (!motion)
        {
            MMM_ERROR << "NULL motion ?!" << endl;
            return false;
        }

        //  if (motion->getJointNames().size()==0)
        //  {
        //      MMM_ERROR << "No Joint name data in motion file" << endl;
        //      return false;
        //  }
        MotionFramePtr motionFrame(new MotionFrame(motion->getJointNames().size()));

        rapidxml::xml_node<>* node = tag->first_node();
        while (node)
        {
            std::string nodeName = XML::toLowerCase(node->name());

            if (xmlProcessorsMotionFrame.find(nodeName) != xmlProcessorsMotionFrame.end())
            {
                if (!xmlProcessorsMotionFrame[nodeName]->processMotionFrameXMLTag(node, motionFrame))
                {
                    MMM_ERROR << "Error while processing XML tag <" << nodeName << ">" << endl;
                    return false;
                }
            }
            else
            {
                //MMM_INFO << "Ignoring unknown motion-data XML tag:" << nodeName << endl;
                // no error: we ignore unknown tags
                //return false;
            }

            node = node->next_sibling();
        }
        return motion->addMotionFrame(motionFrame);
    }


    bool LegacyMotionReaderXML::registerMotionXMLTag(const std::string& xmlTag, XMLMotionTagProcessor* processor)
    {
        // need to store pointers, since we call this method from the constructor (no shared_ptrs available)!
        std::string lc = XML::toLowerCase(xmlTag.c_str());
        XML::toLowerCase(lc);
        xmlProcessors[lc] = processor;
        return true;
    }

    bool LegacyMotionReaderXML::registerMotionXMLTag(const std::string& xmlTag, XMLMotionTagProcessorPtr processor)
    {
        processorList.push_back(processor); // just storing pointer to avoid deletion
        return registerMotionXMLTag(xmlTag, processor.get());
    }

    bool LegacyMotionReaderXML::registerMotionFrameXMLTag(const std::string& xmlTag, XMLMotionFrameTagProcessor* processor)
    {
        // need to store pointers, since we call this method from the constructor (no shared_ptrs available)!
        std::string lc = XML::toLowerCase(xmlTag.c_str());
        XML::toLowerCase(lc);
        xmlProcessorsMotionFrame[lc] = processor;
        return true;
    }

    bool LegacyMotionReaderXML::registerMotionFrameXMLTag(const std::string& xmlTag, XMLMotionFrameTagProcessorPtr processor)
    {
        processorListMotionFrame.push_back(processor); // just storing pointer to avoid deletion
        return registerMotionFrameXMLTag(xmlTag, processor.get());
    }

}
